
/****************************************************************************
*						Adapters Class implementation						*
*    				Copyright (C) 2012  J.R. Ruiz-Sarmiento					*
*																			*
*    This program is free software: you can redistribute it and/or modify	*
*    it under the terms of the GNU General Public License as published by	*
*    the Free Software Foundation, either version 3 of the License, or		*
*    (at your option) any later version.									*
*																			*
*    This program is distributed in the hope that it will be useful,		*
*    but WITHOUT ANY WARRANTY; without even the implied warranty of			*
*    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the			*
*    GNU General Public License for more details.							*
*																			*
*    You should have received a copy of the GNU General Public License		*
*    along with this program.  If not, see <http://www.gnu.org/licenses/>.	*
*																			*
****************************************************************************/

#include "CPointCloudAdapters.h"

using namespace SORT::Adapters;
using namespace SORT::ObjectRecognition;

bool CDataAdapters::PreloadedPbMapToCPlanes( TiXmlDocument &xmlDoc, mrpt::aligned_containers<SORT::ObjectRecognition::CPlane>::vector_t &v_planes )
{
	/* 
	Typical PbMap XML file:

	<?xml version="1.0" ?>
	<PTAM_Xtion version="1.1">
		<MapPlanes size="1">
			<MapPlane id="0" numObservations="15" areaNumVoxels="1.945000" areaHull="2.674481" fullArea="0" ratioXY="1.242099" structure="0" label="P0" normal="0.0227894 -0.214205 0.976274" center="1.05108 0.521125 -0.0716519" PpalComp="0.692366 0.707996 0.139179" RGB="0 0 0">
				<Neighbors size="8">1 2 9 10 11 12 13 15 </Neighbors>
				<CommonObservations>1 1 1 1 1 1 1 1 </CommonObservations>
				<ConvexHull size="5">
					<Vertex pos="-0.200509 -0.0405219 -0.118962" />
					<Vertex pos="-0.198699 -0.0507974 -0.126035" />
					<Vertex pos="-0.0137131 -0.239065 -0.173758" />
					<Vertex pos="1.49231 -0.477926 -0.31539" />
					<Vertex pos="1.63232 -0.470696 -0.318537" />
				</ConvexHull>
			</MapPlane>
		</MapPlanes>
	</PTAM_Xtion>

	*/

	TiXmlNode* p_Parent = &xmlDoc;
	TiXmlNode* p_Child;

	int t = p_Parent->Type();


	if ( t != TiXmlNode::NodeType::TINYXML_DOCUMENT )
	{
		cout << "Error in preloaded PbMap XML format checking if the first node is a document";
		return false;
	}

	p_Child = p_Parent->FirstChild();

	t = p_Child->Type();

	if ( t!= TiXmlNode::NodeType::TINYXML_DECLARATION )
	{
		cout << "Error in preloaded PbMap XML format checking if the first document child is the declaration.";
		return false;
	}

	TiXmlNode* p_PTAM_XtionNode = p_Child->NextSibling();

	t = p_PTAM_XtionNode->Type();

	// strcmp returns a 0 value if the strings are equal
	if ( strcmp(p_PTAM_XtionNode->Value(),"PTAM_Xtion") || ( t !=TiXmlNode::NodeType::TINYXML_ELEMENT ) )
	{
		cout << "Error in preloaded PbMap XML format, PTAM_Xtion node expected.";
		return false;
	}
	
	TiXmlElement* p_MapPlanesElement = p_PTAM_XtionNode->FirstChildElement();
	
	if ( !p_MapPlanesElement || strcmp(p_MapPlanesElement->Value(),"MapPlanes") )
	{
		cout << "Error in preloaded PbMap XML format, MapPlanes node expected.";
		return false;
	}	

	int N_planes;

	p_MapPlanesElement->QueryIntAttribute("size", &N_planes);

	TiXmlElement* p_MapPlaneElement;

	for ( p_MapPlaneElement = p_MapPlanesElement->FirstChildElement(); 
		p_MapPlaneElement; 
		p_MapPlaneElement = p_MapPlaneElement->NextSiblingElement() )
	{
		//id="0" numObservations="15" areaNumVoxels="1.945000" areaHull="2.674481" fullArea="0" ratioXY="1.242099" structure="0" label="P0" normal="0.0227894 -0.214205 0.976274" center="1.05108 0.521125 -0.0716519" PpalComp="0.692366 0.707996 0.139179" RGB="0 0 0">p_MapPlanesElement->QueryIntAttribute("size", &N_planes);

		pcl::PointIndices::Ptr						CPlane_inliers; // Leave empty, NULL, not set
		pcl::PointCloud<pcl::PointXYZ>::VectorType	CPlane_contour;
		pcl::ModelCoefficients::Ptr					CPlane_coeffs( new pcl::ModelCoefficients() );
		Eigen::Vector3f 							CPlane_centroid;
		Eigen::Matrix3f 							CPlane_covariance;
		float										CPlane_elongation;

		int id;

		p_MapPlaneElement->QueryIntAttribute("id", &id);
		//cout << id << endl;

		/* Los vecinos tienen un hijo que es un texto
		const char *message = p_MapPlaneElement->Attribute("Neighbors");*/

		// Get the centroid -> CPlane_centroid
		
		vector<float> v_centroid;
		tokenizeNumericalString( string(p_MapPlaneElement->Attribute("center") ), v_centroid, " ");
		
		CPlane_centroid[0] = v_centroid[0];
		CPlane_centroid[1] = v_centroid[1];
		CPlane_centroid[2] = v_centroid[2];

		// Get the plane coefficients

		vector<float> v_normal;
		tokenizeNumericalString( string(p_MapPlaneElement->Attribute("normal") ), v_normal, " ");
		
		CPlane_coeffs->values.resize(4);
		CPlane_coeffs->values[0] = -v_normal[1];
		CPlane_coeffs->values[1] = -v_normal[2];
		CPlane_coeffs->values[2] = v_normal[0];
		CPlane_coeffs->values[3] = sqrt( pow(v_centroid[0],2) + pow(v_centroid[1],2) + pow(v_centroid[2],2) );

		// Get the vertices (contour points) -> CPlane_contour

		TiXmlElement* p_ConvexHullElement = p_MapPlaneElement->FirstChildElement("ConvexHull"); 

		TiXmlElement* p_VertexElement;
		pcl::PointCloud< pcl::PointXYZ > auxPointCloud;

		for ( p_VertexElement = p_ConvexHullElement->FirstChildElement(); 
			p_VertexElement; 
			p_VertexElement = p_VertexElement->NextSiblingElement() )
		{
			vector<float> v;
			tokenizeNumericalString( string(p_VertexElement->Attribute("pos") ), v, " ");

			pcl::PointXYZ point;
			point.x = v[0];
			point.y = v[1];
			point.z = v[2];

			CPlane_contour.push_back( point );
			auxPointCloud.points.push_back( point );
		}	

		// Compute covariance matrix
		Eigen::Vector4f centroidForCovarianceComptutation;
		centroidForCovarianceComptutation[0] = CPlane_centroid[0];
		centroidForCovarianceComptutation[1] = CPlane_centroid[1];
		centroidForCovarianceComptutation[2] = CPlane_centroid[2];
		centroidForCovarianceComptutation[3] = 0;

		pcl::computeCovarianceMatrixNormalized ( auxPointCloud, centroidForCovarianceComptutation, CPlane_covariance );	

		// Get elongation
		p_MapPlaneElement->QueryFloatAttribute("ratioXY", &CPlane_elongation);

		// Build a new plane and add to the final vector!
		CPlane newPlane( CPlane_inliers, CPlane_coeffs, CPlane_centroid, CPlane_contour, CPlane_covariance, CPlane_elongation );

		v_planes.push_back( newPlane );
	}	

	return true;
}

bool CDataAdapters::PreloadedClassificationResultsToCPlanes ( TiXmlDocument &xmlDoc, mrpt::aligned_containers<SORT::ObjectRecognition::CPlane>::vector_t &v_planes )
{
	TiXmlNode* p_Parent = &xmlDoc;
	TiXmlNode* p_Child;

	/*
	<?xml version='1.0' encoding='UTF-8'?>
	<Processed file path:="E:\Repositorio\Software de desarrollo\Semantic Reasoning and Planning\Semantic Reasoning and Planning\semantic_goal_generation\test_data\detected_planes_5.txt">
	<Detected objects>
		<object type="chair" name="Candidate_chair_4" confidence="1.0" distance_relation_between_parts="0.3" size_relation="1.0" coarse_centroid_height="0.6" object_parts=" 22 21" />
		<surface type="floor" id="9" confidence="1.0" coarse_centroid_height="0.0" />
		<surface type="floor" id="0" confidence="1.0" coarse_centroid_height="0.0" />
		<surface type="floor" id="20" confidence="1.0" coarse_centroid_height="0.0" />
		<surface type="ceiling" id="14" confidence="1.0" coarse_centroid_height="2.0" coarse_area="4.0" />
		<surface type="ceiling" id="15" confidence="1.0" coarse_centroid_height="2.0" coarse_area="3.0" />
		<surface type="wall" id="17" confidence="0.9" coarse_centroid_height="0.8" coarse_area="2.0" />
		<surface type="wall" id="5" confidence="1.0" coarse_centroid_height="1.5" coarse_area="4.0" />
		<surface type="wall" id="16" confidence="0.7" coarse_centroid_height="1.2" coarse_area="1.2" />
		<surface type="wall" id="2" confidence="0.6" coarse_centroid_height="1.5" coarse_area="1.0" />
		<surface type="wall" id="18" confidence="0.8" coarse_centroid_height="1.2" coarse_area="1.5" />
		<surface type="wall" id="4" confidence="0.7" coarse_centroid_height="1.5" coarse_area="1.2" />
		<surface type="wall" id="12" confidence="1.0" coarse_centroid_height="1.0" coarse_area="3.0" />
	</Detected objects>
	</Processed file>
	*/


	int t = p_Parent->Type();


	if ( t != TiXmlNode::NodeType::TINYXML_DOCUMENT )
	{
		cout << "Error in preloaded PbMap XML format checking if the first node is a document";
		return false;
	}

	p_Child = p_Parent->FirstChild();

	t = p_Child->Type();

	if ( t!= TiXmlNode::NodeType::TINYXML_DECLARATION )
	{
		cout << "Error in preloaded PbMap XML format checking if the first document child is the declaration.";
		return false;
	}

	TiXmlNode* p_ProcessedFilenNode = p_Child->NextSibling();

	t = p_ProcessedFilenNode->Type();

	// strcmp returns a 0 value if the strings are equal
	if ( strcmp(p_ProcessedFilenNode->Value(),"ProcessedFile") || ( t !=TiXmlNode::NodeType::TINYXML_ELEMENT ) )
	{
		cout << "Error in preloaded PbMap XML format, Processed file node expected.";
		return false;
	}
	
	TiXmlElement* p_DetectedObjects = p_ProcessedFilenNode->FirstChildElement();
	
	if ( !p_DetectedObjects || strcmp(p_DetectedObjects->Value(),"DetectedObjects") )
	{
		cout << "Error in preloaded PbMap XML format, Detected objects node expected.";
		return false;
	}	

	TiXmlElement* p_DetectedObject;

	for ( p_DetectedObject = p_DetectedObjects->FirstChildElement(); 
		p_DetectedObject; 
		p_DetectedObject = p_DetectedObject->NextSiblingElement() )
	{
		int		id;
		string	type;
		float	confidence;

		if ( !strcmp(p_DetectedObject->Value(),"object") )
		{
			type = p_DetectedObject->Attribute("type");
			p_DetectedObject->QueryFloatAttribute("confidence", &confidence);

			

			if ( type == "chair" )
			{
				// A chair is compound of two planes
				vector<int> v_id;

				tokenizeNumericalString( string(p_DetectedObject->Attribute("object_parts") ), v_id, " ");

				
				for ( size_t index = 0; index < v_id.size(); index++ )
				{ 
					id = v_id[index];

					for ( size_t i = 0; i < v_planes.size(); i++ )
					{
						if ( v_planes[i].getId() == id )
						{
							v_planes[i].setConfidence( confidence );
							v_planes[i].setType ( CClassification::Chair );
							break;
						}
					}
				}
				
			}
			else if ( ( type == "table" ) || ( type == "computer_screen" ) )
			{		
				p_DetectedObject->QueryIntAttribute("id", &id);

				for ( size_t i = 0; i < v_planes.size(); i++ )
				{
					if ( v_planes[i].getId() == id )
					{
						v_planes[i].setConfidence( confidence );					
						if ( type == "table" )
							v_planes[i].setType ( CClassification::Table );
						if ( type == "computer_screen" )
							v_planes[i].setType ( CClassification::Computer_screen );

						break;
					}					
				}
			}

		} else if ( !strcmp(p_DetectedObject->Value(),"surface") )
		{
			p_DetectedObject->QueryIntAttribute("id", &id);
			p_DetectedObject->QueryFloatAttribute("confidence", &confidence);
			type = p_DetectedObject->Attribute("type");

			for ( size_t i = 0; i < v_planes.size(); i++ )
			{
				if ( v_planes[i].getId() == id )
				{
					v_planes[i].setConfidence( confidence );

					if ( type == "wall" )	
						v_planes[i].setType ( CClassification::Wall );
					else if ( type == "ceiling" )	
						v_planes[i].setType ( CClassification::Ceiling );
					else if ( type == "floor" )	
						v_planes[i].setType ( CClassification::Floor );

					break;
				}					
			}
		}
		
	}

	return true;

}

void CDataAdapters::getFeatures( std::string &detection, std::map<std::string, std::string> &features )
{
	vector<string> fields;
	tokenizeString( detection, fields, "," );	

	for ( size_t i = 0; i < fields.size(); i++ )
	{
		vector<string> field; // A string like "feature=value"
		tokenizeString( fields[i], field, "=" );	
	
		features[field[0]] = field[1];	
	}
}

bool CDataAdapters::OnlineClassificationResultsToCPlanes ( std::string res, mrpt::aligned_containers<SORT::ObjectRecognition::CPlane>::vector_t &v_planes )
{
	// Remove the "Ok|" header from string
	res.erase(0,3); //erase(pos,n) Erases a sequence of n characters starting at position pos.

	vector<string> detections;
	tokenizeString( res, detections, "|" );

	for ( size_t i = 0; i < detections.size(); i++ )
	{
		if ( detections[i].size() ) // Check if the current detection has fields
		{
			std::map<std::string, std::string> features;

			getFeatures( detections[i], features );

			string &type = features["type"];

			if ( type == "chair" )
			{
				// A chair is compound of two planes
				vector<int> v_id;

				tokenizeNumericalString( features["object_parts"], v_id, " ");
					
				for ( size_t index = 0; index < v_id.size(); index++ )
				{ 
					size_t id = v_id[index];

					for ( size_t i = 0; i < v_planes.size(); i++ )
					{
						if ( v_planes[i].getId() == id )
						{
							v_planes[i].setConfidence( atoi((features["confidence"]).c_str()) );
							v_planes[i].setType ( CClassification::Chair );
							break;
						}
					}
				}
					
			}
			else if ( ( type == "table" ) || ( type == "computer_screen" ) ||	( type == "wall" )	
						|| ( type == "ceiling" ) || ( type == "floor" )	)
			{
				size_t id = atoi( (features["id"]).c_str() );

				for ( size_t index = 0; index < v_planes.size(); index++ )
				{
					if ( v_planes[index].getId() == id )
					{
						v_planes[index].setConfidence( atoi((features["confidence"]).c_str()) );

						if ( type == "wall" )	
							v_planes[index].setType ( CClassification::Wall );
						else if ( type == "ceiling" )	
							v_planes[index].setType ( CClassification::Ceiling );
						else if ( type == "floor" )	
							v_planes[index].setType ( CClassification::Floor );
						else if ( type == "table" )
							v_planes[index].setType ( CClassification::Table );
						else if ( type == "computer_screen" )
							v_planes[index].setType ( CClassification::Computer_screen );

						break;
					}					
				}
			}
		}
	}

	return true;
}